{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Project2: Inverted Pendulum and Drake Simulation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Derive full state space model \n",
    "\n",
    "state vector: $x = \\begin{bmatrix} z, \\theta, \\dot{z}, \\dot{\\theta} \\end{bmatrix}^T$\n",
    "\n",
    "Continous state space model\n",
    "$$\\dot{x} = f(x, u)$$\n",
    "\n",
    "Continous inverted pendulum cart model $\\dot{x} = f_{\\theta}(x, u)$\n",
    "\n",
    "$$\\dot{x} = \n",
    "\\begin{bmatrix}\n",
    "\\dot{z} \\\\\n",
    "\\dot{\\theta} \\\\\n",
    "\\frac{u + m_p \\sin{\\theta} \\big( L\\dot{\\theta}^2 + g \\cos{\\theta} \\big)}{m_c + m_p \\sin^2{\\theta}} \\\\\n",
    "\\frac{-u \\cos{\\theta} - m_p L \\dot{\\theta}^2 \\cos{\\theta} \\sin{\\theta} - (m_c + m_p) g \\sin{\\theta}}{L (m_c + m_p \\sin^2{\\theta})}\n",
    "\\end{bmatrix}\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Angle dynamic model\n",
    "\n",
    "state vector of inverted pendulum angle: $x_{\\theta} = \\begin{bmatrix} \\theta, \\dot{\\theta} \\end{bmatrix}^T$\n",
    "\n",
    "Continous state space model of inverted pendulum angle\n",
    "$$\\dot{x}_{\\theta} = f_{\\theta}(x_{\\theta}, u)$$\n",
    "\n",
    "$$\\dot{x} = \n",
    "\\begin{bmatrix}\n",
    "\\dot{\\theta} \\\\\n",
    "\\frac{-u \\cos{\\theta} - m_p L \\dot{\\theta}^2 \\cos{\\theta} \\sin{\\theta} - (m_c + m_p) g \\sin{\\theta}}{L (m_c + m_p \\sin^2{\\theta})}\n",
    "\\end{bmatrix}\n",
    "$$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 3. Linearization of angle dynamic model\n",
    "\n",
    "state vector of inverted pendulum angle: $x_{\\theta} = \\begin{bmatrix} \\theta, \\dot{\\theta} \\end{bmatrix}^T$\n",
    "\n",
    "Continous state space model of inverted pendulum angle\n",
    "$$\\dot{x}_{\\theta} = f_{\\theta}(x_{\\theta}, u)$$\n",
    "\n",
    "$$\\dot{x} = \n",
    "\\begin{bmatrix}\n",
    "\\dot{\\theta} \\\\\n",
    "\\frac{-u \\cos{\\theta} - m_p L \\dot{\\theta}^2 \\cos{\\theta} \\sin{\\theta} - (m_c + m_p) g \\sin{\\theta}}{L (m_c + m_p \\sin^2{\\theta})}\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "Linearization around $(\\hat{x}, \\hat{u}) = \\big( \\begin{bmatrix} \\pi & 0 \\end{bmatrix}^T, 0 \\big)$ using Taylor expansion\n",
    "\n",
    "$$ \n",
    "\\tag{1}\n",
    "\\begin{equation}\n",
    "\\begin{split}\n",
    "\\dot{x} & = f_{\\theta}(x, u) \\\\\n",
    "& = f_{\\theta}(\\hat{x}, \\hat{u}) + \\Bigg( \\frac{\\partial f(x, u)}{\\partial x}\\Bigg\\vert_{x = \\hat{x}, u = \\hat{u}} \\Bigg) \\cdot (x - \\hat{x}) + \\Bigg( \\frac{\\partial f(x, u)}{\\partial u}\\Bigg\\vert_{x = \\hat{x}, u = \\hat{u}} \\Bigg) \\cdot (u - \\hat{u}) + H.O.T\n",
    "\\end{split}\n",
    "\\end{equation}\n",
    "$$\n",
    "\n",
    "Superposition following equation into equation (1)\n",
    "$$\n",
    "f_{\\theta} (\\hat{x}_{\\theta}, \\hat{u}) = 0 \\\\\n",
    "\\Delta x_{\\theta} \\triangleq x_{\\theta} - \\hat{x}_{\\theta} = x_{\\theta} \\\\\n",
    "\\Delta u \\triangleq u - \\hat{u} = u\n",
    "$$\n",
    "\n",
    "We can get\n",
    "$$\n",
    "\\dot{x} = \\hat{A} \\cdot x_{\\theta} + \\hat{B} \\cdot u\n",
    "$$\n",
    "\n",
    "and\n",
    "\n",
    "$$\n",
    "\\hat{A} = \\Bigg( \\frac{\\partial f(x, u)}{\\partial x}\\Bigg\\vert_{x = \\hat{x}, u = \\hat{u}} \\Bigg) \\\\\n",
    "\\\\[0.3cm]\n",
    "\\hat{B} = \\Bigg( \\frac{\\partial f(x, u)}{\\partial u}\\Bigg\\vert_{x = \\hat{x}, u = \\hat{u}} \\Bigg)\n",
    "$$\n",
    "\n",
    "The symbol result of matrix $\\hat{A}$ and ${\\hat{B}}$ are calculated in following blocks, where\n",
    "$$\n",
    "\\hat{A} = \\begin{bmatrix} 0 & 1 \\\\ 21.582 & 0 \\end{bmatrix} \\\\[0.3cm]\n",
    "\\hat{B} = \\begin{bmatrix} 0 \\\\ 0.2 \\end{bmatrix}\n",
    "$$\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 232,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sym\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "#define constants\n",
    "t, g = sym.symbols('t g')\n",
    "m_c, m_p = sym.symbols('m_c m_p')\n",
    "L = sym.symbols('L')\n",
    "\n",
    "# define variables \n",
    "u_x = sym.symbols('u_x')\n",
    "theta = sym.symbols(r'\\theta')\n",
    "theta_dot = sym.symbols(r'\\dot{\\theta}')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 233,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "t g m_c m_p L u_x \\theta \\dot{\\theta}\n"
     ]
    }
   ],
   "source": [
    "print(t, g, m_c, m_p, L, u_x, theta, theta_dot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 234,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\dot{\\theta}\\\\\\frac{- L \\dot{\\theta}^{2} m_{p} \\sin{\\left(\\theta \\right)} \\cos{\\left(\\theta \\right)} - g \\left(m_{c} + m_{p}\\right) \\sin{\\left(\\theta \\right)} - u_{x} \\cos{\\left(\\theta \\right)}}{L \\left(m_{c} + m_{p} \\sin^{2}{\\left(\\theta \\right)}\\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[                                                                                                                 \\dot{\\theta}],\n",
       "[(-L*\\dot{\\theta}**2*m_p*sin(\\theta)*cos(\\theta) - g*(m_c + m_p)*sin(\\theta) - u_x*cos(\\theta))/(L*(m_c + m_p*sin(\\theta)**2))]])"
      ]
     },
     "execution_count": 234,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "z_2dot = (u_x + m_p * sym.sin(theta) * (L * (theta_dot) ** 2 + g * sym.cos(theta))) / (m_c + m_p * sym.sin(theta) ** 2)\n",
    "theta_2dot = (- u_x * sym.cos(theta) - m_p * L * (theta_dot) ** 2 * sym.cos(theta) * sym.sin(theta) - (m_c + m_p) * g * sym.sin(theta)) / (L * (m_c + m_p * (sym.sin(theta)) ** 2))\n",
    "f_theta = sym.Matrix([theta_dot, theta_2dot])\n",
    "\n",
    "f_theta"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 235,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 1\\\\- \\frac{2 m_{p} \\left(- L \\dot{\\theta}^{2} m_{p} \\sin{\\left(\\theta \\right)} \\cos{\\left(\\theta \\right)} - g \\left(m_{c} + m_{p}\\right) \\sin{\\left(\\theta \\right)} - u_{x} \\cos{\\left(\\theta \\right)}\\right) \\sin{\\left(\\theta \\right)} \\cos{\\left(\\theta \\right)}}{L \\left(m_{c} + m_{p} \\sin^{2}{\\left(\\theta \\right)}\\right)^{2}} + \\frac{L \\dot{\\theta}^{2} m_{p} \\sin^{2}{\\left(\\theta \\right)} - L \\dot{\\theta}^{2} m_{p} \\cos^{2}{\\left(\\theta \\right)} - g \\left(m_{c} + m_{p}\\right) \\cos{\\left(\\theta \\right)} + u_{x} \\sin{\\left(\\theta \\right)}}{L \\left(m_{c} + m_{p} \\sin^{2}{\\left(\\theta \\right)}\\right)} & - \\frac{2 \\dot{\\theta} m_{p} \\sin{\\left(\\theta \\right)} \\cos{\\left(\\theta \\right)}}{m_{c} + m_{p} \\sin^{2}{\\left(\\theta \\right)}}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[                                                                                                                                                                                                                                                                                                                           0,                                                                      1],\n",
       "[-2*m_p*(-L*\\dot{\\theta}**2*m_p*sin(\\theta)*cos(\\theta) - g*(m_c + m_p)*sin(\\theta) - u_x*cos(\\theta))*sin(\\theta)*cos(\\theta)/(L*(m_c + m_p*sin(\\theta)**2)**2) + (L*\\dot{\\theta}**2*m_p*sin(\\theta)**2 - L*\\dot{\\theta}**2*m_p*cos(\\theta)**2 - g*(m_c + m_p)*cos(\\theta) + u_x*sin(\\theta))/(L*(m_c + m_p*sin(\\theta)**2)), -2*\\dot{\\theta}*m_p*sin(\\theta)*cos(\\theta)/(m_c + m_p*sin(\\theta)**2)]])"
      ]
     },
     "execution_count": 235,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Matrix A\n",
    "pf_theta_px = f_theta.jacobian([theta, theta_dot])\n",
    "pf_theta_px"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 236,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0\\\\- \\frac{\\cos{\\left(\\theta \\right)}}{L \\left(m_{c} + m_{p} \\sin^{2}{\\left(\\theta \\right)}\\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[                                          0],\n",
       "[-cos(\\theta)/(L*(m_c + m_p*sin(\\theta)**2))]])"
      ]
     },
     "execution_count": 236,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Matrix B\n",
    "pf_theta_pu = f_theta.jacobian([u_x])\n",
    "pf_theta_pu"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 237,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.     1.   ]\n",
      " [21.582 -0.   ]]\n",
      "[[0. ]\n",
      " [0.2]]\n"
     ]
    }
   ],
   "source": [
    "pf_theta_px_f = sym.lambdify((g, u_x, m_c, m_p, L, theta, theta_dot), pf_theta_px)\n",
    "pf_theta_pu_f = sym.lambdify((g, u_x, m_c, m_p, L, theta, theta_dot), pf_theta_pu)\n",
    "\n",
    "A_hat = pf_theta_px_f(9.81, 0, 10, 1, 0.5, np.pi, 0)\n",
    "B_hat = pf_theta_pu_f(9.81, 0, 10, 1, 0.5, np.pi, 0)\n",
    "print(A_hat)\n",
    "print(B_hat)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. Discrete linearized angle dynamic model\n",
    "\n",
    "According to the formula of discrete-time model\n",
    "$$\n",
    "\\begin{split}\n",
    "A & = I + \\hat{A} \\cdot \\Delta t \\\\\n",
    "& = \\begin{bmatrix} 1 & 0.005 \\\\ 0.10791 & 1 \\end{bmatrix} \\\\[0.5cm]\n",
    "\n",
    "B & = \\hat{B} \\cdot \\Delta \\\\\n",
    "& = \\begin{bmatrix} 0 \\\\ 0.001 \\end{bmatrix}\n",
    "\\end{split}\n",
    "$$\n",
    "\n",
    "Finally we have the discrete time model:\n",
    "\n",
    "$$\n",
    "\\begin{split}\n",
    "x_{\\theta} (k + 1) & = \\begin{bmatrix} 1 & 0.005 \\\\ 0.10791 & 1 \\end{bmatrix} \\cdot x_{\\theta}(k) + \\begin{bmatrix} 0 \\\\ 0.001 \\end{bmatrix} \\cdot u(k) \\\\\n",
    "y(k) &= \\begin{bmatrix} 1 \\\\ 0 \\end{bmatrix} x_{\\theta}(k)\n",
    "\\end{split}\n",
    "$$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5. Drake simulation setup and testing"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 248,
   "metadata": {},
   "outputs": [],
   "source": [
    "server_args = []\n",
    "\n",
    "import math\n",
    "import numpy as np\n",
    "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
    "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)\n",
    "\n",
    "from pydrake.all import (LeafSystem,BasicVector,DiagramBuilder, AddMultibodyPlantSceneGraph, Parser, LinearQuadraticRegulator, LogOutput,\n",
    "                         Simulator, RigidTransform, CoulombFriction, FindResourceOrThrow, DrakeVisualizer, ConnectContactResultsToDrakeVisualizer,\n",
    "                         RollPitchYaw, JointIndex, namedview, ConnectMeshcatVisualizer,\n",
    "                         Value, List, ZeroOrderHold, SpatialAcceleration, RotationMatrix, AbstractValue, ConstantVectorSource)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Firsly**, set the initial state of the cart system to $x_{\\theta} = \\begin{bmatrix} \\pi + 0.3 \\\\ 0.1 \\end{bmatrix}$  \n",
    "Simulate the system with the figures shown below the code block"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 239,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Connecting to meshcat-server at zmq_url=tcp://127.0.0.1:6006...\n",
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7006/static/\n",
      "Connected to meshcat-server.\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "<pydrake.systems.analysis.SimulatorStatus at 0x7f26523c8fb0>"
      ]
     },
     "execution_count": 239,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "builder = DiagramBuilder()\n",
    "# First add the cart-pole system from a urdf file\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)\n",
    "urdf_path = \"./urdfExample_cart_pole.urdf\"    \n",
    "cart_pole = Parser(plant, scene_graph).AddModelFromFile(urdf_path)    \n",
    "plant.Finalize()\n",
    "\n",
    "# Add controller (u = 0)\n",
    "controller = builder.AddSystem(ConstantVectorSource([0]))\n",
    "\n",
    "# connect to make diagram\n",
    "builder.Connect(controller.get_output_port(), plant.get_actuation_input_port())\n",
    "\n",
    "# set up visualization using meshcat\n",
    "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, open_browser=True)\n",
    "diagram = builder.Build()\n",
    "\n",
    "# start simulation\n",
    "UprightState = np.array([0, np.pi, 0, 0])   # the state of the cart-pole is organized as [z, zdot, theta, theta_dot]\n",
    "simulator = Simulator(diagram)\n",
    "simulator.set_target_realtime_rate(1)\n",
    "context = simulator.get_mutable_context()\n",
    "context.SetContinuousState(UprightState + np.array([0.0, 0.3, 0.0, 0.1]))\n",
    "\n",
    "simulator.Initialize()\n",
    "sim_time = 5\n",
    "meshcat.start_recording()\n",
    "simulator.AdvanceTo(sim_time)\n",
    "meshcat.stop_recording()\n",
    "meshcat.publish_recording()\n",
    "simulator.AdvanceTo(sim_time)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Result**:\n",
    "This Inverted pendulum system are oscillating form left top position to right top position"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<center>\n",
    "    <img src=\"./imgs/free_fall_1_0.png\" width=50%><img src=\"./imgs/free_fall_1_1.png\" width=50%>\n",
    "    <img src=\"./imgs/free_fall_1_2.png\" width=50%><img src=\"./imgs/free_fall_1_3.png\" width=50%>\n",
    "</center>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Secondly**, set the initial state of this system to $x_{\\theta} = \\begin{bmatrix} \\pi + 0.3 \\\\ 2 \\end{bmatrix}$   \n",
    "with a bigger initial angular speed of the pendulum"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 240,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Connecting to meshcat-server at zmq_url=tcp://127.0.0.1:6006...\n",
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7006/static/\n",
      "Connected to meshcat-server.\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "<pydrake.systems.analysis.SimulatorStatus at 0x7f2652539cb0>"
      ]
     },
     "execution_count": 240,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "builder = DiagramBuilder()\n",
    "# First add the cart-pole system from a urdf file\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)\n",
    "urdf_path = \"./urdfExample_cart_pole.urdf\"    \n",
    "cart_pole = Parser(plant, scene_graph).AddModelFromFile(urdf_path)    \n",
    "plant.Finalize()\n",
    "\n",
    "# Add controller (u = 0)\n",
    "controller = builder.AddSystem(ConstantVectorSource([0]))\n",
    "\n",
    "# connect to make diagram\n",
    "builder.Connect(controller.get_output_port(), plant.get_actuation_input_port())\n",
    "\n",
    "# set up visualization using meshcat\n",
    "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, open_browser=True)\n",
    "diagram = builder.Build()\n",
    "\n",
    "# start simulation\n",
    "UprightState = np.array([0, np.pi, 0, 0])   # the state of the cart-pole is organized as [z, zdot, theta, theta_dot]\n",
    "simulator = Simulator(diagram)\n",
    "simulator.set_target_realtime_rate(1)\n",
    "context = simulator.get_mutable_context()\n",
    "context.SetContinuousState(UprightState + np.array([0.0, 0.3, 0.0, 2]))\n",
    "\n",
    "simulator.Initialize()\n",
    "sim_time = 5\n",
    "meshcat.start_recording()\n",
    "simulator.AdvanceTo(sim_time)\n",
    "meshcat.stop_recording()\n",
    "meshcat.publish_recording()\n",
    "simulator.AdvanceTo(sim_time)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Result**:\n",
    "This Inverted pendulum system can rotate around the hinge which with a higher initial speed than previous one."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<center>\n",
    "    <img src=\"./imgs/free_fall_2_0.png\" width=50%><img src=\"./imgs/free_fall_2_1.png\" width=50%>\n",
    "    <img src=\"./imgs/free_fall_2_2.png\" width=50%><img src=\"./imgs/free_fall_2_3.png\" width=50%>\n",
    "</center>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 6. Simulation studies for state-feedback control "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### (a) Eigenvalue assignment"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can directly get the result of matrix $K$ by the method provided by package *scipy.signal* using the eigenvalues we required  \n",
    "Where $K = \\begin{bmatrix} 132.66140054 & 19.9250837 \\end{bmatrix}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 241,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "matrix([[132.66140054,  19.9250837 ]])"
      ]
     },
     "execution_count": 241,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T = 0.005\n",
    "A_d = np.mat(\"1, 0.005; 0.10791, 1\")\n",
    "B_d = np.mat(\"0; 0.001\")\n",
    "C_d = np.mat(\"1, 0\")\n",
    "s_desired = np.array([-2 + 1j, -2 - 1j])\n",
    "z_desired = np.exp(s_desired * T)\n",
    "\n",
    "import scipy.signal as sig\n",
    "K = sig.place_poles(A_d, B_d, z_desired).gain_matrix\n",
    "K"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### (b) Closed-loop Simulation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We created a controller named ```LinearStateFeedbackCartController```, which using the matrix $K$ to control the inverted pendulum system"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 242,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Connecting to meshcat-server at zmq_url=tcp://127.0.0.1:6006...\n",
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7006/static/\n",
      "Connected to meshcat-server.\n"
     ]
    },
    {
     "ename": "AttributeError",
     "evalue": "'VectorLogSink_[float]' object has no attribute 'sample_times'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mAttributeError\u001b[0m                            Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-242-6b01c0b70f81>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m     56\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     57\u001b[0m \u001b[0mplt\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mfigure\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 58\u001b[0;31m \u001b[0mplt\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mplot\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mlogger_input\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msample_times\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mlogger_input\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mdata\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mtranspose\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     59\u001b[0m \u001b[0mplt\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mxlabel\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m't'\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     60\u001b[0m \u001b[0mplt\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mylabel\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m'y(t)'\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mAttributeError\u001b[0m: 'VectorLogSink_[float]' object has no attribute 'sample_times'"
     ]
    },
    {
     "data": {
      "text/plain": [
       "<Figure size 432x288 with 0 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "class LinearStateFeedbackCartController(LeafSystem):\n",
    "    def __init__(self, update_T, K):\n",
    "        LeafSystem.__init__(self)\n",
    "        num_state = 2\n",
    "        num_input = 4\n",
    "        num_output = 1\n",
    "        self.K = K\n",
    "        self.DeclareDiscreteState(num_state)\n",
    "        self.DeclareVectorInputPort(\"u\", BasicVector(num_input))\n",
    "        self.DeclareVectorOutputPort(\"y\", BasicVector(num_output), self.CalcOutputY)\n",
    "        self.DeclarePeriodicDiscreteUpdate(update_T)\n",
    "\n",
    "    def CalcOutputY(self, context, output):\n",
    "        state_x = self.get_input_port(0).Eval(context)\n",
    "        state_x = state_x - np.array([0, np.pi, 0, 0])\n",
    "        x_theta = np.array([state_x[1], state_x[3]])\n",
    "        y = - np.dot(self.K, x_theta)\n",
    "        output.SetFromVector(y.reshape(-1, ))\n",
    "\n",
    "T = 0.005\n",
    "\n",
    "builder = DiagramBuilder()\n",
    "# First add the cart-pole system from a urdf file\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)\n",
    "urdf_path = \"./urdfExample_cart_pole.urdf\"    \n",
    "cart_pole = Parser(plant, scene_graph).AddModelFromFile(urdf_path)    \n",
    "plant.Finalize()\n",
    "\n",
    "# Add controller (u = 0)\n",
    "controller = builder.AddSystem(LinearStateFeedbackCartController(0.005, K[0]))\n",
    "\n",
    "# connect to make diagram\n",
    "builder.Connect(plant.get_state_output_port(), controller.get_input_port(0))\n",
    "builder.Connect(controller.get_output_port(), plant.get_actuation_input_port())\n",
    "logger_input = LogOutput(controller.get_output_port(0), builder)\n",
    "logger_output = LogOutput(plant.get_state_output_port(), builder)\n",
    "\n",
    "# set up visualization using meshcat\n",
    "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, open_browser=True)\n",
    "diagram = builder.Build()\n",
    "\n",
    "# start simulation\n",
    "UprightState = np.array([0, np.pi, 0, 0])   # the state of the cart-pole is organized as [z, zdot, theta, theta_dot]\n",
    "simulator = Simulator(diagram)\n",
    "simulator.set_target_realtime_rate(1)\n",
    "context = simulator.get_mutable_context()\n",
    "context.SetContinuousState(UprightState + np.array([0.0, 0.5, 0.0, 0.0]))  \n",
    "simulator.Initialize()\n",
    "\n",
    "sim_time = 5\n",
    "meshcat.start_recording() \n",
    "simulator.AdvanceTo(sim_time)\n",
    "meshcat.stop_recording()\n",
    "meshcat.publish_recording()\n",
    "simulator.AdvanceTo(sim_time)\n",
    "\n",
    "plt.figure()\n",
    "plt.plot(logger_input.sample_times(), logger_input.data().transpose())\n",
    "plt.xlabel('t')\n",
    "plt.ylabel('y(t)')\n",
    "plt.show()\n",
    "\n",
    "plt.figure()\n",
    "plt.plot(logger_output.sample_times(), logger_output.data().transpose()[:, [1,3]])\n",
    "plt.xlabel('t')\n",
    "plt.ylabel('x_theta(t)')\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Result Analysis\n",
    "The simulation result is unclear under web-base GUI simulation, but the result plot clearly depict the **result**\n",
    "The cart initially with a very large force. When the pendulum are more close to the equilibrum. cart can be stable with a constant velocity  \n",
    "**Note**: data figure 2 yellow curve is **angular speed curve**, blue is **angle curve**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<center>\n",
    "    <img src=\"./imgs/linear_feedback_1_0.png\" width=100%>\n",
    "    <img src=\"./imgs/linear_feedback_1_1.png\" width=100%>\n",
    "    <img src=\"./imgs/linear_feedback_1_2.png\" width=100%>\n",
    "    <img src=\"./imgs/u_change_1.png\" ><img src=\"./imgs/x_theta_change_1.png\">\n",
    "</center>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 7. Repeat steps in problem 6 with new eigenvalues "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "matrix([[444.48486861,  21.48420104]])"
      ]
     },
     "execution_count": 59,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T = 0.005\n",
    "A_d = np.mat(\"1, 0.005; 0.10791, 1\")\n",
    "B_d = np.mat(\"0; 0.001\")\n",
    "C_d = np.mat(\"1, 0\")\n",
    "s_desired = np.array([-2 + 8j, -2 - 8j])\n",
    "z_desired = np.exp(s_desired * T)\n",
    "\n",
    "import scipy.signal as sig\n",
    "K = sig.place_poles(A_d, B_d, z_desired).gain_matrix\n",
    "K"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/leitianjian/Documents/code/drake-build/install/lib/python3.6/site-packages/pydrake/common/cpp_template.py:392: DrakeDeprecationWarning: (Deprecated.)\n",
      "\n",
      "Deprecated:\n",
      "    Use LogVectorOutput instead. This will be removed from Drake on or\n",
      "    after 2021-12-01.\n",
      "  def f(*args, **kwargs): return orig(*args, **kwargs)\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Connecting to meshcat-server at zmq_url=tcp://127.0.0.1:6005...\n",
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7005/static/\n",
      "Connected to meshcat-server.\n"
     ]
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "class LinearStateFeedbackCartController(LeafSystem):\n",
    "    def __init__(self, update_T, K):\n",
    "        LeafSystem.__init__(self)\n",
    "        num_state = 2\n",
    "        num_input = 4\n",
    "        num_output = 1\n",
    "        self.K = K\n",
    "        self.DeclareDiscreteState(num_state)\n",
    "        self.DeclareVectorInputPort(\"u\", BasicVector(num_input))\n",
    "        self.DeclareVectorOutputPort(\"y\", BasicVector(num_output), self.CalcOutputY)\n",
    "        self.DeclarePeriodicDiscreteUpdate(update_T)\n",
    "\n",
    "    def CalcOutputY(self, context, output):\n",
    "        state_x = self.get_input_port(0).Eval(context)\n",
    "        state_x = state_x - np.array([0, np.pi, 0, 0])\n",
    "        x_theta = np.array([state_x[1], state_x[3]])\n",
    "        y = - np.dot(self.K, x_theta)\n",
    "        output.SetFromVector(y.reshape(-1, ))\n",
    "\n",
    "T = 0.005\n",
    "\n",
    "builder = DiagramBuilder()\n",
    "# First add the cart-pole system from a urdf file\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)\n",
    "urdf_path = \"./urdfExample_cart_pole.urdf\"    \n",
    "cart_pole = Parser(plant, scene_graph).AddModelFromFile(urdf_path)    \n",
    "plant.Finalize()\n",
    "\n",
    "# Add controller (u = 0)\n",
    "controller = builder.AddSystem(LinearStateFeedbackCartController(0.005, K[0]))\n",
    "\n",
    "# connect to make diagram\n",
    "builder.Connect(plant.get_state_output_port(), controller.get_input_port(0))\n",
    "builder.Connect(controller.get_output_port(), plant.get_actuation_input_port())\n",
    "logger_input = LogOutput(controller.get_output_port(0), builder)\n",
    "logger_output = LogOutput(plant.get_state_output_port(), builder)\n",
    "\n",
    "# set up visualization using meshcat\n",
    "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, open_browser=True)\n",
    "diagram = builder.Build()\n",
    "\n",
    "# start simulation\n",
    "UprightState = np.array([0, np.pi, 0, 0])   # the state of the cart-pole is organized as [z, zdot, theta, theta_dot]\n",
    "simulator = Simulator(diagram)\n",
    "simulator.set_target_realtime_rate(1)\n",
    "context = simulator.get_mutable_context()\n",
    "context.SetContinuousState(UprightState + np.array([0.0, 0.5, 0.0, 0.0]))  \n",
    "simulator.Initialize()\n",
    "\n",
    "sim_time = 5\n",
    "meshcat.start_recording() \n",
    "simulator.AdvanceTo(sim_time)\n",
    "meshcat.stop_recording()\n",
    "meshcat.publish_recording()\n",
    "simulator.AdvanceTo(sim_time)\n",
    "\n",
    "plt.figure()\n",
    "plt.plot(logger_input.sample_times(), logger_input.data().transpose())\n",
    "plt.xlabel('t')\n",
    "plt.ylabel('y(t)')\n",
    "plt.show()\n",
    "\n",
    "plt.figure()\n",
    "plt.plot(logger_output.sample_times(), logger_output.data().transpose()[:, [1,3]])\n",
    "plt.xlabel('t')\n",
    "plt.ylabel('x_theta(t)')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Result Analysis\n",
    "By adjusting the eigenvalue of the control system, this inverted pendulum system can be stable more **quickly**, but with a big **overshoot**  \n",
    "**Note**: data figure 2 yellow curve is **angular speed curve**, blue is **angle curve**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<center>\n",
    "    <img src=\"./imgs/linear_feedback_2_0.png\" width=100%>\n",
    "    <img src=\"./imgs/linear_feedback_2_1.png\" width=100%>\n",
    "    <img src=\"./imgs/linear_feedback_2_2.png\" width=100%>\n",
    "    <img src=\"./imgs/linear_feedback_2_3.png\" width=100%>\n",
    "    <img src=\"./imgs/u_change_2.png\" width=50%><img src=\"./imgs/x_theta_change_2.png\" width=50%>\n",
    "</center>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 8. Ouput feedback control design"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### (a) Observer design"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 252,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[0.08810064]\n",
      " [0.51427411]]\n"
     ]
    }
   ],
   "source": [
    "T = 0.005\n",
    "A_d = np.mat(\"1, 0.005; 0.10791, 1\")\n",
    "B_d = np.mat(\"0; 0.001\")\n",
    "C_d = np.mat(\"1, 0\")\n",
    "s_desired = np.array([-2 + 1j, -2 - 1j])\n",
    "z_desired = np.exp(s_desired * T)\n",
    "\n",
    "import scipy.signal as sig\n",
    "K = sig.place_poles(A_d, B_d, z_desired).gain_matrix\n",
    "\n",
    "obs_eig_s = np.array([-9 + 2j, -9 - 2j])\n",
    "obs_eig_z = np.exp(obs_eig_s * T)\n",
    "Ktilde = sig.place_poles(A_d.T, C_d.T, obs_eig_z)\n",
    "L = Ktilde.gain_matrix.T\n",
    "print(L)\n",
    "\n",
    "class LinearStateFeedbackCartController2(LeafSystem):\n",
    "    def __init__(self, update_T, K):\n",
    "        LeafSystem.__init__(self)\n",
    "        num_state = 2\n",
    "        num_input = 2\n",
    "        num_output = 1\n",
    "        self.K = K\n",
    "        self.DeclareDiscreteState(num_state)\n",
    "        self.DeclareVectorInputPort(\"u\", BasicVector(num_input))\n",
    "        self.DeclareVectorOutputPort(\"y\", BasicVector(num_output), self.CalcOutputY)\n",
    "        self.DeclarePeriodicDiscreteUpdate(update_T)\n",
    "\n",
    "    def CalcOutputY(self, context, output):\n",
    "        state_x = self.get_input_port(0).Eval(context)\n",
    "        state_x = state_x - np.array([np.pi, 0])\n",
    "        y = - np.dot(self.K, state_x)\n",
    "        output.SetFromVector(y.reshape(-1, ))\n",
    "\n",
    "# Define the system.\n",
    "class DTObserver(LeafSystem):\n",
    "    def __init__(self, A, B, C, L, T):\n",
    "        LeafSystem.__init__(self)\n",
    "        n = A.shape[0]\n",
    "        m = B.shape[1]\n",
    "        p = 4   \n",
    "        self.A = A\n",
    "        self.B = B\n",
    "        self.C = C\n",
    "        self.L = L     \n",
    "        self.DeclareDiscreteState(n)\n",
    "        # Define the input\n",
    "        self.DeclareVectorInputPort(\"uk\", BasicVector(m))\n",
    "        self.DeclareVectorInputPort(\"yk\", BasicVector(p))\n",
    "        # Define the output\n",
    "        self.DeclareVectorOutputPort(\"x_estimated\", BasicVector(n), self.CalcOutputY, set([self.all_state_ticket()]))\n",
    "        self.DeclarePeriodicDiscreteUpdate(T)  # One second timestep.         \n",
    "\n",
    "    def DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):\n",
    "        xk = context.get_discrete_state_vector().CopyToVector()\n",
    "        uk = self.get_input_port(0).Eval(context)\n",
    "        yk = self.get_input_port(1).Eval(context)\n",
    "        # print(yk)\n",
    "        yk = yk - np.array([0, np.pi, 0, 0])\n",
    "        xk = xk - np.array([np.pi, 0])\n",
    "        yk = yk[1]\n",
    "        # print(yk, xk)\n",
    "        A = self.A \n",
    "        B = self.B \n",
    "        C = self.C\n",
    "        L = self.L\n",
    "        xk = xk.reshape((2,1))\n",
    "        # print(L.shape)\n",
    "        x_next = np.dot(A, xk) + B * uk + L * (yk - np.dot(C, xk)) + np.array([np.pi, 0]).reshape((2,1))\n",
    "        # print(x_next)\n",
    "        discrete_state.get_mutable_vector().SetFromVector(x_next)\n",
    "        \n",
    "    def CalcOutputY(self, context, output):\n",
    "        x = context.get_discrete_state_vector().CopyToVector()\n",
    "        y = x\n",
    "        output.SetFromVector(y)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### (b) Simulation result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 255,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/leitianjian/Documents/code/drake-build/install/lib/python3.6/site-packages/pydrake/common/cpp_template.py:392: DrakeDeprecationWarning: (Deprecated.)\n",
      "\n",
      "Deprecated:\n",
      "    Use LogVectorOutput instead. This will be removed from Drake on or\n",
      "    after 2021-12-01.\n",
      "  def f(*args, **kwargs): return orig(*args, **kwargs)\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Connecting to meshcat-server at zmq_url=tcp://127.0.0.1:6007...\n",
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7007/static/\n",
      "Connected to meshcat-server.\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "Text(0, 0.5, 'x_theta(t)')"
      ]
     },
     "execution_count": 255,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAXkAAAEGCAYAAACAd+UpAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjMuNCwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy8QVMy6AAAACXBIWXMAAAsTAAALEwEAmpwYAAApLklEQVR4nO3de3xcdZ3/8dfnzH1yT5umbdqS0hul5V4u/kBuCoKCICqKoqKsuKvuettdF/fiz139qevKD1ddFVB2df2JKCoIi2xhgQqUS7kVKKW0JW3TtE3S5p7M9Xx/f0xaGtImaZvJtCfvJ495TGa+58z5nCa858x3vud7zDmHiIgEk1fqAkREpHgU8iIiAaaQFxEJMIW8iEiAKeRFRAIsXOoC9jZ16lTX2NhY6jJERI4oTz/9dLtzrm5fbYdVyDc2NrJq1apSlyEickQxs037a1N3jYhIgCnkRUQCTCEvIhJgCnkRkQBTyIuIBJhCXkQkwBTyIiIBdliNky+WvO/446ttrN3eg+8cs2uSnHH0FOoqYqUuTUSkqAIT8hu7NnL3hrsZyA2QzqfJuzy+8zm15nJuvLeHje27wBz4cQBCnnH+MdP4xNlHs6yxtsTVi4gUR2BCvrmnmZ+8+BMS4QTRUJSwFyad9fnlA7Opj8/nmgu7+O2WGzlu6vEcX3MO/buW8PtnOnjPD1fy1sX1/O07FjN3almpd0NEZFzZ4XRlqGXLlrmDndZg936YGQBrWrq54gePsrC+gp9dezqtqSbua7qPB7c8yLqOdYS9MBfMeRsN+Q9zy8NbSOd8Pvymo/jixccQCemrChE5cpjZ0865ZftsC0rI7y2b91n21ftxzrH88+dQXxkf0r6xcyO/fOWXNHU38aMLfkRrd4q/+NUKHn81y/Gzqvj2e09gQX3FIdchIjIRRgr5wHTX7O23z26layDLP12+dFjAAxxdfTTXn37960f/4R42xv+ON73pFNauOZN3fLeHv7xwIdeedTQhzya6fBGRcRO4folc3uffHlzPcQ1VXH36nBGX3d21kwgnuGbJNbzW9yz+zG8xe8Hv+fp/P8H7frSSjW29E1G2iEhRBC7k7395B007+/nUefP2hPhoyqPlfPLET3LvFffywWM/SIf3ODULb+CVtm1c/J0/cvOKjeT9w6dbS0RkrAIX8r9+upn6yhgXHDv9gNetidfw16f+Nb9/1++5/owv8sBn38GbF9TxjRV3cMUPH2J9a08RKhYRKZ5AhXx7b5qHXmnj8pMaDqkvvaG8gfcufC/TKuP8/eV1JGf/lI2xf+CSW2/kO/evJZ3Lj2PVIiLFE6iQv+u5FnK+4z0nzxq31zyq6ih+fOGPWVQ3k8j02/nRhk9z/g/+jUfXt43bNkREiiVQIf/A2h0srC8f9+GPp804jV9dehvfPufbTK8O013x71x96/9wza1P8lTTrnHdlojIeArMEMq+dI6nXuvgmjMbi/L6ZsaFjRdy3pzzWL1jDQ83JPjZ401cfecXWFC+jL8//72cPndaUbYtInKwAhPyj23YSSbvc+7CfV6wfNxEvAinzDiBU2bAe0+r4v13/zObcqv42P23MY3zufbE93DlSUuIhgP1IUlEjlCBSaJ/feBVktHQhE42NruqnhVX3cc3z/o2sytn0h79Dd946WrO+L838c0/rGXt9u4Jq0VEZF8CcSSfzuV5YWsXC+vLJ/wIOuSFePu8C3n7vAtZ37GBHz79K3ZyHDet2Mgtq2+lqqaZM+rP5gPHXcQZR83B0xm0k5Jzjrzv8B34zjGQHSCTz5L1c+R8n7zLE7II5ZFKfN+xo38HWT+H7zt855N3PjEvSVWsFt93bOlpIufy+M6R931850iGK6iNTcN3jo3dL+MA5wpt4KiKTmVqfAZ5l2Nd52ochdfePbPJ1MQM6mIzyfoZXul6Dgafd4O3+vhsauPTSeX6Wd+9ek9b4d4xMzGX6ug0+rO9bOx9cc9+715uZmI+VdGp9GY7aOp9edi/0ZyyY6iI1NCd3cnmvrXD2ueWL6UsXEVHppXmvnV7trvb/IqTSITL2ZluYWv/+mHrL6o8lVgoQWtqM9sHmgbre719cdUZRLwo2wdeozW1Zdj6S6rPJGQhWvo30J7e+sbfMMfVnA1Ac/86OtLbh7R6FmJJ9ZkAbOpbQ1dm6MCNiBfjPYvfVpRJEgMR8pmcD8CVy2aXtI75NfP4l7f+DVAYzvmVh1/hkdaVPNzxPR5e8T0sM4OjEsv40KI/46Q51cyrK1foH4S870hl83SnU3Sn++lJ99Ob7qcvM8D05FzSWZ/13WvZ3reVdC5LOp8hnc+S9x2Lyy4kk/dZ2/NH2tKvkfWzZP0sOT+H52LMDb2fXN6nKf87etxG8i6LI49PjpBfzbSBj5PzHW3xW8mENuEsXwga52PZGXitf0Iu73Azvg+xrYAP5mPmk+ubx8DmjwNQNu+f8aJDv7TP9iwm1fyRQvuCf8IL9w1t7zqRVMv7AShf9HeYlxvSnuk4nfT2dwE+FYu/NOzfLbPzbNKtbwdvgIpFXxnWnm67gEz7W7BwF+ULvj6sPbX9ErIdZ+FFWymbd8Pw9m1XkO08DS++hbK53x/WPtB8FbmeEwgl15M86pZh7f2bryHfdwzh8pdIzP7Z8PamT5AfmEu48lkSDb8c1t638S/w0zOJVD9OfMbvhrX3rv9LXHYq0dqHidXfO7x93d/i8hVE6/6b2NT/Gdbes/YfwUWJTfs90SmPDmlzzui9tzDgIzbj10Srh87B5fIxetcV/s3jDf+PSOXqIe1+tpLHXqjnt588c9h2D1UgJijL+47X2nupSUaZUn54XQjEOcdjW57n9jXLea71aTr6QnRv+gAAFY03UxFLMqtsAQtr5nF8/TzOmH0Ms6trx3y27uHGOUcq69OdStEx0EdXqo/OVN9gGPdRG5lLLh+mubeJpt41DOQG6M+mSOcHSOVTzOKdZHNRtuVW0s5Kcn6GPGl80vhkyGz6HOlsmNi0u4lOeWTY9nte/jpgxKb/hmjNk0Nry0fpXfePAMRn/oJw5QuYCwFhzIXw/GpqO79IyDN6y39NJrwRI4xHGM9CxKij0X2ESMjYyu9I2w48C+GZR8hClIXqOTbxLkKesT59D2nXSci8PctUhWdwTPl5mBmv9N1P1g3s1W5UR6Yzr/wUzIy1PQ+TJ4tnHh6GZx61sek0li/BM+Olzj9i5jDz8MwImceU+HRmly/AOceazifwzDA8zAzPjCmx6dQnZuOTZ333C3hY4T8r3Gpj9UyJTyPnZ9jc+yoY7P4rNGBqYjo1salk8hma+zbsed4GD1Tq4jOoitaQzvfT0r95TzuDf8vTkzOpiFTRn+tjx0Azb/wTr080UBYppy/bQ+vAtmG/2+nJWSTCSXqz3exMte6pzLA97bFQjJ5MFx3p9j3rmb3eHvGidGc66coMHxXXUDaHkBemM72L7kznsPZZ5Y145tGRaqcnO7wr9qiKowHYmWqjLzv0xEnPPGaVNwLQnmplIDf0DXxnb47TZi3iqCkHdyQ/6WahPJzl8z6v7ezn2c0d3Lrum2xPvUrG245Z4dNIpnMZ4Z3vo74qSrr2x1REqqmJT6E2NoWaRDXzqxaysHYBZVFjV6aFylg5iUiEeDhCPBKhIpokEorg+27wKDVPzs+Rdz65fI6wFwMXpTc9QEvvNnozA/RnB+jPpOjLpaiLziURqmV7Xwurdz1KKpdiIDdAKp8mnU9Tz/lYdjrtuTW02F3kXJq8KwSwsywDWz5MPjWLSNVTxGfeMWz/+zZ8Dj9TT6TmUeLTfz+00Y9Q0/F3lIXqyCQepTv6R8IWI2Ixol6MqBfnTVWfoDJWTlvueXbmNpAIJ0iE4yTCCZLhBCfXvZl4JExPro2cGyAZiZGMRElEYyTDUerKaomGPSKe4XmB+UpKJjmF/GFuIJvimZb1PN2ynu7eGPnUHJo7O3gm+3WydOF7PXveBNJtbyXT/tb9f6Te8Q6yu968/4/ULe8m23UqXnwzZXP/bXgt+/tI7YfBRanovoYqWwrxDXTE7iRiMSJenKgXIxZKcGLl5UxPzqbfNdOSfp5kNEF5JEFZNElFLMkJU0+kNlFJnn5y1k9VLEl5NEk8HMczha7IwShZyJtZHFgBxCj0///aOffl/S0/WUN+NHk/z9bunbT07MLlE5Avp72vm+d3PUZ/rp9MLkvOz5PJ56iPHktdbB5pv5s1Pf+9p6ugcB/i6LLjmVk2lyw9NPU9QzKcIBGJk4zESUYSzKmcw5RENeGwwyxNVTxJZSyho16Rw1gp55NPA+c753rNLAI8Ymb3OuceL/J2AyXkhZhTPY051XufbDWFS5k7yponj9K+5BArE5HDXVFD3hU+JuyekD0yeDt8+odERAKu6J/BzSxkZs8BrcBy59wTb2i/zsxWmdmqtjZN+iUiMp6KHvLOubxz7kRgFnCamS19Q/tNzrllzrlldXXFnZJARGSymbBv05xzncCDwEUTtU0RkcmuqCFvZnVmVj34cwK4ABh+vrKIiBRFsUfXzAD+w8xCFN5QbnfO3V3kbYqIyKBij65ZDZxUzG2IiMj+6QwXEZEAU8iLiASYQl5EJMAU8iIiAaaQFxEJMIW8iEiAKeRFRAJMIS8iEmAKeRGRAFPIi4gEmEJeRCTAFPIiIgGmkBcRCTCFvIhIgCnkRUQCTCEvIhJgCnkRkQBTyIuIBJhCXkQkwBTyIiIBppAXEQkwhbyISIAp5EVEAkwhLyISYAp5EZEAU8iLiASYQl5EJMAU8iIiAaaQFxEJMIW8iEiAKeRFRAJMIS8iEmAKeRGRAFPIi4gEmEJeRCTAFPIiIgGmkBcRCTCFvIhIgBU15M1stpk9aGZrzOwlM/tMMbcnIiJDhYv8+jngC865Z8ysAnjazJY759YUebsiIkKRj+Sdc9ucc88M/twDvAw0FHObIiLyugnrkzezRuAk4Ik3PH+dma0ys1VtbW0TVY6IyKQwISFvZuXAHcBnnXPde7c5525yzi1zzi2rq6ubiHJERCaNooe8mUUoBPzPnXO/Kfb2RETkdcUeXWPAj4GXnXM3FHNbIiIyXLGP5M8EPgScb2bPDd7eXuRtiojIoKIOoXTOPQJYMbchIiL7pzNeRUQCTCEvIhJgBxTyZlZmZqFiFSMiIuNrxJA3M8/MPmBm95hZK7AW2DY4F823zGz+xJQpIiIHY7Qj+QeBecD1wHTn3Gzn3DTgLOBx4JtmdnWRaxQRkYM02uiatzrnsm980jm3i8IJTncMnuwkIiKHoRGP5HcHvJn97I1tu5/b15uAiIgcHsb6xeuSvR8Mfvl6yviXIyIi42m0L16vN7Me4Hgz6x689QCtwJ0TUqGIiBy00bprvu6cqwC+5ZyrHLxVOOemOOeun6AaRUTkII12JN8IsL9At4JZRahLRETGwWija75lZh6FrpmngTYgDswHzgPeAnwZaC5mkSIicnBGDHnn3HvN7Fjgg8DHgBnAAIXL+N0DfM05lyp6lSIiclBGnYVy8KLbfzsBtYiIyDgb81TDZrYUOJZCdw0AzrmfFqMoEREZH2MKeTP7MnAuhZD/L+Bi4BFAIS8ichgb68lQ76HwJet259xHgROAqqJVJSIi42KsIT/gnPOBnJlVUjgZanbxyhIRkfEw1j75VWZWDdxMYShlL7CyWEWJiMj4GFPIO+c+OfjjD83sD0Clc2518coSEZHxMKbuGjN7YPfPzrkm59zqvZ8TEZHD04hH8mYWB5LAVDOrAWywqRJoKHJtIiJyiEbrrvkE8FlgJvDMXs93A98rUk0iIjJORpvW4DvAd8zsz51z352gmkREZJyMdQjlT8zs78zsJgAzW2BmlxSxLhERGQdjDnkgA/yvwcdbga8WpSIRERk3Yw35ec65fwayAM65fl7/ElZERA5TYw35jJklAAdgZvOAdNGqEhGRcTHWM16/DPwBmG1mPwfOBK4pVlEiIjI+xnrG63IzewY4g0I3zWecc+1FrUxERA7ZmOeTpzCPfMfgOseaGc65FcUpS0RExsNY55P/JvA+4CXAH3zaAQp5EZHD2FiP5C8HFjnn9GWriMgRZKyjazYCkWIWIiIi42+0Ccq+S6Fbph94bnDmyT1H8865vyhueSIicihG665ZNXj/NHDXG9rc+JcjIiLjabQJyv4DwMw+MzhZ2R5m9pliFiYiIodurH3yH9nHc9eMYx0iIlIEo/XJXwV8AJhrZnt311QAu0Z7cTP7CXAJ0OqcW3oohYqIyIEbrU/+MWAbMBX49l7P9wBjucbrv1O4uMhPD6Y4ERE5NKP1yW8CNgFvGmk5M1vpnBu2jHNuhZk1HlKFIiJy0MbaJz+a+MGuaGbXmdkqM1vV1tY2TuUAuQxkU+P3eiIiR6DxCvmDHk7pnLvJObfMObesrq5ufKrJpuDG4+DrDfDiHePzmiIiR6DxCvnDy/rl0Lsd/Bws/zLks6WuSESkJMYU8mZ27D6eO3fvh+NUz/hYcyckp8CVP4WuLbD+gVJXJCJSEmM9kr/dzL5oBYnB6Q6+vlf7h/a1kpn9AlgJLDKzZjO79hDrHZvmVdB4Fiy8GGKV8Mo9E7JZEZHDzVhD/nRgNoUhlU8BLRSuDgWAc+7Ffa3knLvKOTfDORdxzs1yzv34UAseVaoLOl6D6cdDOArzzodX7wenWRhEZPIZa8hngQEgQWEkzWvOOX/kVUpk++D7zYwTCveNZ0FPC3RuKl1NIiIlMtaQf4pCyJ8KvBm4ysx+VbSqDsWuDYX7qQsL93MGh+9vfrw09YiIlNBYQ/5a59w/OOeyzrltzrnLGD4r5eGhczNYCCobCo+nLYZYFWxeWdq6RERKYEwh75xbtY/nfjb+5YyDzs2FgA8NnszrhWDWMtjyVGnrEhEpgeCNk+/cDNVzhj7XcDK0rYVMf2lqEhEpkWCFvO8XumWqZg19fsaJ4PKwY5+DgEREAitYIZ/qLNwnaoY+P/PEwn3LcxNYjIhI6QUr5PsHp7ifedLQ5ysbIFEL28cyO7KISHAEK+QHOgr3ydqhz5vB9KWw46WJr0lEpISCGfJv7K4BqF9a+PLVz09sTSIiJRSwkB/srtlXyE87FrL90NE0oSWJiJRSwEJ+pCP5wYk01WUjIpNIsEK+fxdgEK8a3la3uNDWumaiqxIRKZlghfxABySqC2e5vlE0CbVzdSQvIpNKAEN+H101u9Uv0ZG8iEwqAQv5XYXx8PszbQns2qjpDURk0ghYyI92JH8sOL8wlFJEZBIIVsinuvb9petu05YU7tVlIyKTRLBCPt0DsYr9t9fOBUxj5UVk0phcIe+FIFoGL/9+4moSESmh4IR8Pgu5FMQqR16uYvrrE5mJiARccEI+3VO4j5WPvNwpH4W+VuhtK35NIiIlFsCQH6G7Bgpj5QFadVKUiARfcEI+01u4j45yJF+/tHCvM19FZBIITsiP9Ui+vA7KpinkRWRSmHwhD4UuG13vVUQmgckb8q1rIZ8rbk0iIiU2SUN+KeTTsGtDcWsSESmx4IT8WL94hddH2KhfXkQCLjghfyBH8nWLwEIKeREJvGCFfCS57wuGvFE4BlMXKuRFJPCCFfJjOYrfrX6JQl5EAi9YIT+W/vjd6pdA1+bC9MQiIgEVnJDf+NABHskPnvm6XePlRSS4ghHy+Vzh0n+7v3wdi5knFu5bni1KSSIih4NghLzzC/cnXjX2dcqnQeUsaHmmODWJiBwGghXydoC703ASbFXIi0hwBSPkcQD0PLuJzdddR/cf7hvbajNPho7XdBGRg+Ec+D74+UJ3WT4LuczgLV249/1SVyky6YWLvQEzuwj4DhACbnHOfWPcN+IcuQGPrbc8iMvkyO/cRfm55+DF4yOv13By4b7lWZj/lnEv67DnHPS147duILtuNdnmJnKtO8h3dpLv7qF6SZxovI/e9d20P9mHy/m4nMPlHH7eMfvsXcSrc3S+lmDHM8MvoN54QRuxKkfHxjLaXyjDQoUPWxbysLAx+9JywlVJutbl6V47gEXCeNEwFo3gxaPUXbIUL1lG/2vdpLb34iUSeIkElijDEknKTl6KRZPkejP4ecMrr8KSlYX7eDmE4xAq+p+4yGGtqP8HmFkI+D5wAdAMPGVmdznn1ozvlhwdG5K4bI65d91JbN48LDSGk6JmnFi4b3km2CGfz+I3ryb99ArSa1eT2fgamZZ2auftIjmlj/6WGFtWTBm6jkFySjXRY6Zj1WVYohUvEsaLRrBIpBDEZ54AdVVE57RSVdY0bLOhc5dCeYRIzRbK2ITL5XG5HC6bw+Xy2NQ5EM3j53aQ7c7gcilc1scffCOpm/MyWJruJ6N0rHvj8FjHMe/bBgZtT1bRubFsSKsX9Vl0xXbwwmx/ppq+bREsbHhhD4uECFdEaLi8ASIJdq3qJLMrh0WjWCyKF4sRrqmk+tzjIZKg7+UW8mmHJZJ4iTIsWUaoqpbY/HkQjpPr7AUvgsUSEItjsQQWS2LhCJiN7+9S5AAV+zDnNGC9c24jgJndBlwGjG/IO5/ebXHijdOIL1wIQL6nB5fNEq6t3f96iWqYMh+2BmyETaqbzBN3QsuzRFNrSb/0HBvvqQQ3GDgeRGvj5BsvhrNOI+6qaDing/BRCwjPmkdoah1eMol5hd68MqDs+v1vLnkuJD+2//by82GkMxhqPgg1I7RP6+9natcu/L4uXG8Xfl8PbqAPW3wUZFNUnbKGxKZm/IF+3EA/fmoA/ByctxSyA0QHXiIXa8NlsrhMFj+bx89RuCbwQAcDr3bStyVXeHPJA86IVWWpzv4KgLblUxnYGR1SU2JKhsYL2gHYfG8d6a7IkPay+jRzztsFoQgb76khN+CBB+YZFoLyo8JMP68cQhE2/64blwXCXuFTTihE2dGV1L6pHrwwLXdsLLxZeB54hnkeZQumUnnSLJwzWu9eO/h8CEIe5oVILppB2eJZ+DmfjgdfhlBosD0EnkdiwSwSc6eTT2XoeeKVwY9YNrgdI7FgNrGZdeT7U/Q+u77w+nstE58/m2j9FHI9/Qy83DS4XmhPe2L+HMK11eS6e0mt3wKeV1jfK7xGfN4cQpXl5Dp7yTRvH/ZmGJs7m1BZglxHF5mW1sKTey0TnzcHLx4ju7OL7I72oX8wZsTnH4UXjZBt30WudXd37F7rL5qLhUJk23aR29X5hr84I75wLuZ5ZHe0k+vo2nvVwr/fwrmAkdnWSr6nb+ja4RDxeUcBkGnZQb63f8i2LRImPnc2mJHZuh1/IFXY5wULsemLGW/FDvkGYMtej5uB08d7I873KZ+RIrLsWAD8VIr1b72AqsveyfQvfWnklWeeBE2PjHdJE87fspq+3/2Y3scep+/VXWR7w9Qs6mf6JfOInnM1UyJdxE8+g9iJZxJtnItFXg+lMFB5RulqH42XTOIlk8CsfbYnG88kOcL6tW+FEd7qafjToY9dLodLDUAEyKZoeF8T+a4OXH8Prr8PP9WLFzZYMANyKabWrybf2YPLZXHZwi1Sk4BlsyGfpWLnC+R7+l//JJPLE52ZgNqp4OewyEDh+UweP5/D5X3y1Rlo7wY/z0BTPy7vcA7wwTlHaGAzlZHHcRmfzkeShTbH4L0xZdNjlG3vwU95tP5u+rB9rju+m8SxveR7Q2y7u35Ye/0pncQW9JPtCNNy37Rh7TNO7yA6d4BMW5TmB6YO/zc9cxeVs1Ok9vUpEZh97k7Kp6fp3xxn62PDfzuNb20jMTVL78YE254cfghw9MWtxKpy9Kwr22dX4fxLd+CV5el6qZy2FyqHtS981zZCMUfH8xXsfHn4+TXHXNkCHuxcVUXH+qGfEs1zHHPlNgDaH6+mq2noX18olmfhu3YA0PpIDT3NiSHtkbIc8y8tvHFtf3AKfTtihZr+tIbQZx8bVsuhMufcuL/onhc3ew9wkXPuTwYffwg43Tn36b2WuQ64DmDOnDmnbNq06cA3lM/Cuvtg2mKYMg+A5j//cwaee575Dz+054h0nx7/Afzhb+DzL0PlzAPfdil1bIIX78Ct/hUbftJOti+MRaBs0UzKzjqb8kuvIjpvYamrlBJw+TyGj8vncP29uGwW/CwuW/iS3ItF8ZJxXDZHrr0dfB/n8oV3ibxPqLqCUFkSP5Umu31H4Ut0V7g53xGpqyFUXobf30+muQXnD74D5fPgIDpzGqGKMvLdPaQ3by287uCX9c7PE5/bQKg8Sa6ji3TT1jcUD/GFRxEqS5Bt7yS9ZRu7B1fsljxmbuFIvnUX6S3bh+1/cuk8vFiUzLY2Mi1tb1ydsuMXYpEQ6S07hn4SGMzDspMXY55HalML2dahAzPMjPJTCkfcqY3NZNs7h7aHPcpPLrQPrNtErqN7aHs0QvmJhf8v+19+jXxX4ZNA+ZmnYQvO2+fvczRm9rRzbtk+24oc8m8C/rdz7m2Dj68HcM59fV/LL1u2zK1atWpctt111120/PUXabztFyROPHH/CzavglveAu/9D1hy+bhsu6h8n+zK2+m49fsMvLKVOefvxOacTlfXsYSWnE/ZeRdh0ejoryMigTFSyBe7u+YpYIGZzQW2Au8HPlDkbQJQfs45EA7Tc//9I4f89OMhFIPmpw7vkM+lSf3uBtpv/U96NhaOlsqXHo3/0bsINS5l+AdWEZEih7xzLmdmnwbuozCE8ifOuQmZ+jFUVUXZ6afTs/x+6r7wBWx/oxzCUZhxQiHkD0e5DDz7M/pu+zab73F4UWPKZWdT88kvEZnTWOrqROQwV/RBxM65/wL+q9jb2Ze6z39uyBeM+zX7NHjy5kKghg+Trg7nyK68ncxvv0ZZ2SaSi06lfuYZVP3JXxGq0nG7iIxNoM8USSxZMrYFZ50KK78H21+AWacUt6gxcNtfYedXPk77ih2E4iHm/+w2bPFF1GrMtYgcoIBMa7B/fStX0n7zzSMvNPu0wv3mlcUvaCR+ntRt/0DTFZfQ9mAb5SfM56g77sGOvVgn1YjIQQl+yD+2krbv/Cv5nhGmIa6cWZiR8qFv7BlCNeHaXiHzL+fx2j/eTjYdp+GbX2HW/7uHaOPc0tQjIoEQ+JAvP+9cyOXoe/TRkRc89jLI9ED7uoko63XO4Vb9FH50DlG3henXvpOjl6+g8rIrJ7YOEQmkwId84oQTCFVV0fvgQyMv+KZPFe5fmcDviNO99N94FRv+9J9IRY6DT66k5gv/TLh2pJP8RUTGLvAhb6EQZWefTe+KFbh8fv8LVjUUhlK+cu/EFNa+nq6/PItNNz8HiRp453egYvjp5yIihyLwIQ8Uph1OJMhtH3768xCL3g5bnoSeHUWtx716P22fehsty9MkjzuGuXcvJ7742KJuU0Qmp0kR8pUXXcS8B+4n0tAw8oJLrgAcvPjr4hTiHDz+A7r+6cO0Px+l6h0XMOdnt2vcu4gUTaDHye+2e2555/sjT1ZWtxAaToHnf/F6H/14GeiEe78Iq2+j6m1vx3vPlVRcevn+z8QVERkHk+JIHqD34Yd59c1nkx2ty+aEqwonRW1/Yfw2PtCB/9Mr2XbLvWSP+yR21c+pfOe7FPAiUnSTJuQjDQ3kd+6k96GHR15w6bsLl4176pbx2XBHE/5NF9D8iw10vlbGQNk5hQsniIhMgEmTNtF584jMmkXvQw+NvGCyFk54Pzx/26Ff4HvzE7gfvYWWe7vo2x5lxle/RuVFFx3aa4qIHIBJE/JmRvm559L3+OP4qdTIC5/+Z4VLw/3x2we/wedvw/37JWx/KkFPU5hpf/VXVL/7ioN/PRGRgzBpQh6g/NxzcakU/U88MfKC046BuWfDkzdB+6sHtpFsCu77W/jtJ/DrT2Ug3ciUP7mWKdeOcBFUEZEimRSja3ZLnnYqNR/+EOEZM0Zf+N0/hu8tg7s/Bx++a2z96H3tcPtHYNMjuJM+TOiSG2i8OoMlR7oCqYhI8UyqI3kvGmX6l75EfOEYrntaPg0u/Bo0/RHu/4fRl1//APzwLGh+iq6yq9i6HPxsHq+sTKNoRKRkJlXIQ+FK9/3PPkumqWn0hU+6Gk79ODz2XVj+5cIFw99o5wb45YfgP6+AWAW9x/8LLf/xKPmuLk0PLCIlN6m6awD8vj42f/RjVF12GTO+8r9HXtgMLvoG+Fl49MbC5GUnvB+qZkPvDtjwIGz4n8KQy/P/nv7EWTR//M+ILVzArB/8G14sNhG7JCKyX5Mu5EPl5VS+7UK677mH+r/5Il4iMcoKYbjkRph/AfzxX+CBf3y9raYRzvkiLPsoqW3dbLn6Q0Tq65lz882EysuLuRsiImMy6UIeoOqKd9N151303H8/VZdeOvoKZrD4ksKtf1fhlqiGsql7FnFNbURmzGDW979PeMqU4hUvInIAJl2fPEDy1GVE5syh4z9/jjvQK0Ela2Hq/D0Bv3vMfeK445j7298QnTXKJGgiIhNoUoa8eR6113yEzKZN5HYc/LTC+a4umt73ftp/dNOe1xUROZxMyu4agOp3v5vqyy/HO8gx7H5fH1uu+wSZjRuJL10yztWJiIyPSXvo6cVieMkkLpcj29JyQOv6AwNs+dSnGXjxRWbe8G3KzzyzSFWKiByaSRvyuzV/5rNsvu46/ExmTMs732fLx6+j/8knmfl/vkblBRcUuUIRkYM36UO+5sr3klm/gdZvfHNMy5vnUfWud9Fwww1UXXZZkasTETk0k7ZPfrfyc86h9qMfZdettxI9+mhqr/7gPpfrXbECP52m8oILNJukiBwxJn3IA0z7/OfIbN7Mjq9+FZfLMuWaa4DCFAipF15g58230LN8OYkTTqDiLW/RKBoROWIo5AGLRJh14/+l7V+/S8X55wPQdeedbP/a/8Hv7saSSeo++1lqP/ZRBbyIHFEU8oMsEmHaFz6/53GouprKt19M4rjjqHjb2zRNgYgckRTy+1F+zjmUn3NOqcsQETkk6nsQEQkwhbyISIAp5EVEAkwhLyISYAp5EZEAU8iLiASYQl5EJMAU8iIiAWYHfPm7IjKzNmDTIbzEVKB9nMo5Uky2fZ5s+wva58niUPb5KOdc3b4aDquQP1Rmtso5t6zUdUykybbPk21/Qfs8WRRrn9VdIyISYAp5EZEAC1rI31TqAkpgsu3zZNtf0D5PFkXZ50D1yYuIyFBBO5IXEZG9KORFRAIsECFvZheZ2Stmtt7M/qbU9UwEM/uJmbWa2YulrmUimNlsM3vQzNaY2Utm9plS11RsZhY3syfN7PnBff5KqWuaKGYWMrNnzezuUtcyEcysycxeMLPnzGzVuL72kd4nb2YhYB1wAdAMPAVc5ZxbU9LCiszMzgZ6gZ8655aWup5iM7MZwAzn3DNmVgE8DVwe5N+zmRlQ5pzrNbMI8AjwGefc4yUurejM7PPAMqDSOXdJqespNjNrApY558b9BLAgHMmfBqx3zm10zmWA24DLSlxT0TnnVgC7Sl3HRHHObXPOPTP4cw/wMtBQ2qqKyxX0Dj6MDN6O7KOyMTCzWcA7gFtKXUsQBCHkG4Atez1uJuD/8092ZtYInAQ8UeJSim6w2+I5oBVY7pwL/D4DNwJ/DfglrmMiOeC/zexpM7tuPF84CCEvk4iZlQN3AJ91znWXup5ic87lnXMnArOA08ws0F1zZnYJ0Oqce7rUtUyws5xzJwMXA58a7I4dF0EI+a3A7L0ezxp8TgJmsF/6DuDnzrnflLqeieSc6wQeBC4qcSnFdibwzsE+6tuA883sP0tbUvE557YO3rcCv6XQDT0ughDyTwELzGyumUWB9wN3lbgmGWeDX0L+GHjZOXdDqeuZCGZWZ2bVgz8nKAwuWFvSoorMOXe9c26Wc66Rwv/L/+Ocu7rEZRWVmZUNDibAzMqAC4FxGzV3xIe8cy4HfBq4j8KXcbc7514qbVXFZ2a/AFYCi8ys2cyuLXVNRXYm8CEKR3bPDd7eXuqiimwG8KCZraZwMLPcOTcphhROMvXAI2b2PPAkcI9z7g/j9eJH/BBKERHZvyP+SF5ERPZPIS8iEmAKeRGRAFPIi4gEmEJeRCTAFPIiozCzajP7ZKnrEDkYCnmR0VUDCnk5IinkRUb3DWDe4AlY3yp1MSIHQidDiYxicNbLuyfDvP0SPDqSFxEJMIW8iEiAKeRFRtcDVJS6CJGDoZAXGYVzbifwqJm9qC9e5UijL15FRAJMR/IiIgGmkBcRCTCFvIhIgCnkRUQCTCEvIhJgCnkRkQBTyIuIBNj/B+3OEZQ7b6u2AAAAAElFTkSuQmCC",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "T = 0.005\n",
    "# Create a simple block diagram containing our system.\n",
    "builder = DiagramBuilder()\n",
    "# First add the cart-pole system from a urdf file\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)\n",
    "urdf_path = \"./urdfExample_cart_pole.urdf\"    \n",
    "cart_pole = Parser(plant, scene_graph).AddModelFromFile(urdf_path)    \n",
    "plant.Finalize()\n",
    "\n",
    "controller = builder.AddSystem(LinearStateFeedbackCartController2(0.005, K[0]))\n",
    "my_observer = builder.AddSystem(DTObserver(A_d, B_d, C_d, L, T))\n",
    "\n",
    "builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port()) \n",
    "builder.Connect(controller.get_output_port(0), my_observer.get_input_port(0))\n",
    "builder.Connect(plant.get_state_output_port(), my_observer.get_input_port(1))\n",
    "builder.Connect(my_observer.get_output_port(0), controller.get_input_port(0))\n",
    "\n",
    "logger_plant = LogOutput(plant.get_state_output_port(), builder)\n",
    "logger_estimate = LogOutput(my_observer.get_output_port(0), builder)\n",
    "\n",
    "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, open_browser=True)\n",
    "diagram = builder.Build()\n",
    "\n",
    "simulator = Simulator(diagram)\n",
    "context = simulator.get_mutable_context()  #overall context\n",
    "\n",
    "UprightState = np.array([0, np.pi, 0, 0])   # the state of the cart-pole is organized as [z, zdot, theta, theta_dot]\n",
    "\n",
    "plant_context = diagram.GetMutableSubsystemContext(plant, context)\n",
    "plant_context.SetContinuousState(UprightState + np.array([0.0, 0.3, 0.0, 0.0]))  \n",
    "observer_context = diagram.GetMutableSubsystemContext(my_observer, context)\n",
    "observer_context.SetDiscreteState([np.pi, 0])\n",
    "\n",
    "# start simulation\n",
    "simulator.set_target_realtime_rate(1)\n",
    "simulator.Initialize()\n",
    "\n",
    "sim_time = 5\n",
    "meshcat.start_recording() \n",
    "simulator.AdvanceTo(sim_time)\n",
    "meshcat.stop_recording()\n",
    "meshcat.publish_recording()\n",
    "simulator.AdvanceTo(sim_time)\n",
    "\n",
    "\n",
    "plt.figure()\n",
    "plt.plot(logger_estimate.sample_times(), logger_estimate.data().transpose())\n",
    "plt.plot(logger_plant.sample_times(), logger_plant.data().transpose()[:, [1,3]], \"--\")\n",
    "plt.xlabel('t')\n",
    "plt.ylabel('x_theta(t)')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Result Analysis\n",
    "Applying Observer to this system and get following result  \n",
    "**Note**: data figure 1 dot line show the estimation of observer of angle and angular velocity."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<center>\n",
    "    <img src=\"./imgs/observer_1.png\" width=100%>\n",
    "    <img src=\"./imgs/observer_2.png\" width=100%>\n",
    "    <img src=\"./imgs/estimate_gt.png\" >\n",
    "</center>"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6"
  },
  "kernelspec": {
   "display_name": "Python 3.6.9 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
